#include "Triangle.h"
#include "math/Matrix4.h"

#include <cstdio>

//------------------------------------------------------------------------------

Triangle::Triangle(Particle* p1, Particle* p2, Particle* p3)
{
	p[0] = p1;
	p[1] = p2;
	p[2] = p3;
	updateAABB();
	oldAabb = aabb;
}

//------------------------------------------------------------------------------

void Triangle::updateAABB()
{
	oldAabb = aabb;
	aabb.corner1 = aabb.corner2 = p[0]->position;
	for(int i = 0; i < 3; ++i) {
		Vector3& pos = p[i]->position;
		if(pos.x < aabb.corner1.x)
			aabb.corner1.x = pos.x;
		if(pos.x > aabb.corner2.x)
			aabb.corner2.x = pos.x;
		if(pos.y < aabb.corner1.y)
			aabb.corner1.y = pos.y;
		if(pos.y > aabb.corner2.y)
			aabb.corner2.y = pos.y;
		if(pos.z < aabb.corner1.z)
			aabb.corner1.z = pos.z;
		if(pos.z > aabb.corner2.z)
			aabb.corner2.z = pos.z;
	}
	aabb.center = (aabb.corner1+aabb.corner2)/2;
}

/*
 * Check whether the particle p has collided with this triangle during the
 * last time step.
 * Appropriate response is generated.
 * Note: we detect collision between a moving particle and a static triangle
 * (we take the position of the triangle at the end of the time step). This
 * simplifies the algorithm.
 * See Moore 1988.
 */
bool Triangle::collide(Particle* P)
{
	//fprintf(stderr, "Triangle::collide\n");
	if(P == p[0] || P == p[1] || P == p[2])
		return false;
	/*
	 * Collision detection.
	 * We solve the equation P + (P'-P)*t = P0 + (P1-P0)*u + (P2-P0)*v for
	 * t, u and v
	 */
	Vector3 tCoeff = P->position - P->oldPosition;
	Vector3 uCoeff = -(p[1]->position - p[0]->position);
	Vector3 vCoeff = -(p[2]->position - p[0]->position);
	Vector3 freeCoeff = p[0]->position - P->oldPosition;
	// we ignore the last row and column
	Matrix4 M(tCoeff, uCoeff, vCoeff);
	Matrix4 Minv; // inverse of M
	//M.print();
	if(!M.Inverse2(Minv))
		return false;
	//fprintf(stderr, "Triangle::collide INVERSE\n");
	// Compute the solution
	//M = Minv*M; M.print();
	Vector3 tuv = Minv*freeCoeff;
	double t = tuv.x;
	double u = tuv.y;
	double v = tuv.z;
	if(!(0 <= t && t <= 1 && u >= 0 && v >= 0 && u+v <= 1))
		return false;
	// We have a collision!
	fprintf(stderr, "Triangle::collide COLLISION!\n");
	return true;
	
	/*
	 * Collision response.
	 * We move the particle towards the intersection point.
	 */
	Vector3 dir = (p[0]->position+uCoeff*u+vCoeff*v) - P->position;
	double totalMass = p[0]->invMass+p[1]->invMass+p[2]->invMass+P->invMass;
	// This shouldn't happen, but we treat it nonetheless.
	if(totalMass == 0)
		return true;
	P->position = (p[0]->position+uCoeff*u+vCoeff*v);
	/*
	P->position += (P->invMass/totalMass)*dir;
	p[0]->position -= (p[0]->invMass/totalMass)*dir;
	p[1]->position -= (p[1]->invMass/totalMass)*dir;
	p[2]->position -= (p[2]->invMass/totalMass)*dir;
	*/
	return true;
}
